#include "BLDCDriver3PWM.h"
#include "soc/soc_gpio.h"

#define digitalWrite SOC_Gpio::Write
#define GPIOPORTPIN_IS_SET(p) !(p.IsNone()) 

BLDCDriver3PWM::BLDCDriver3PWM(const GpioPortPin &pinA, const GpioPortPin &pinB, const GpioPortPin &pinC, 
                               const GpioPortPin &en1, const GpioPortPin &en2, const GpioPortPin &en3)
{
  // Pin initialization
  pwmA = pinA;
  pwmB = pinB;
  pwmC = pinC;

  // enable_pin pin
  enableA_pin = en1;
  enableB_pin = en2;
  enableC_pin = en3;

  // default power-supply value
  voltage_power_supply = DEF_POWER_SUPPLY;
  voltage_limit = NOT_SET;
  pwm_frequency = NOT_SET;

}

void BLDCDriver3PWM::attachTimer(IF_SOC_Timer *ta, IF_SOC_Timer *tb, IF_SOC_Timer *tc, int cha, int chb, int chc)
{
  timerA = ta;
  timerB = tb;
  timerC = tc;

  pwmACh = cha;
  pwmBCh = chb;
  pwmCCh = chc;
}

// enable motor driver
void  BLDCDriver3PWM::enable(){
    // enable_pin the driver - if enable_pin pin available
    if ( GPIOPORTPIN_IS_SET(enableA_pin) ) digitalWrite(enableA_pin, enable_active_high);
    if ( GPIOPORTPIN_IS_SET(enableB_pin) ) digitalWrite(enableB_pin, enable_active_high);
    if ( GPIOPORTPIN_IS_SET(enableC_pin) ) digitalWrite(enableC_pin, enable_active_high);
    // set zero to PWM
    setPwm(0,0,0);
}

// disable motor driver
void BLDCDriver3PWM::disable()
{
  // set zero to PWM
  setPwm(0, 0, 0);
  // disable the driver - if enable_pin pin available
  if ( GPIOPORTPIN_IS_SET(enableA_pin) ) digitalWrite(enableA_pin, !enable_active_high);
  if ( GPIOPORTPIN_IS_SET(enableB_pin) ) digitalWrite(enableB_pin, !enable_active_high);
  if ( GPIOPORTPIN_IS_SET(enableC_pin) ) digitalWrite(enableC_pin, !enable_active_high);

}

// init hardware pins
int BLDCDriver3PWM::init() {
  // PWM pins
  // pinMode(pwmA, OUTPUT);
  // pinMode(pwmB, OUTPUT);
  // pinMode(pwmC, OUTPUT);
  // if( GPIOPORTPIN_IS_SET(enableA_pin)) pinMode(enableA_pin, OUTPUT);
  // if( GPIOPORTPIN_IS_SET(enableB_pin)) pinMode(enableB_pin, OUTPUT);
  // if( GPIOPORTPIN_IS_SET(enableC_pin)) pinMode(enableC_pin, OUTPUT);


  // sanity check for the voltage limit configuration
  if(!_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;

  // Set the pwm frequency to the pins
  // hardware specific function - depending on driver and mcu
  // params = _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC);
  // initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED);
  // return params!=SIMPLEFOC_DRIVER_INIT_FAILED;

  initialized =true;
  return true;
}

// Set voltage to the pwm pin
void BLDCDriver3PWM::setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) {
  // disable if needed
  if( GPIOPORTPIN_IS_SET(enableA_pin) &&  GPIOPORTPIN_IS_SET(enableB_pin)  && GPIOPORTPIN_IS_SET(enableC_pin) ){
    digitalWrite(enableA_pin, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high);
    digitalWrite(enableB_pin, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high);
    digitalWrite(enableC_pin, sc == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high);
  }
}

// Set voltage to the pwm pin
void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) {
#if SVPWM_OPTIMIZE_EN > 0 
  if(timerA != nullptr)
  {
    timerA->SetPwmDutyP(pwmACh, Ua);
  }
  if(timerB != nullptr)
  {
    timerB->SetPwmDutyP(pwmBCh, Ub);
  }
  if(timerC != nullptr)
  {
    timerC->SetPwmDutyP(pwmCCh, Uc);
  }
#else 

  // limit the voltage in driver
  Ua = _constrain(Ua, 0.0f, voltage_limit);
  Ub = _constrain(Ub, 0.0f, voltage_limit);
  Uc = _constrain(Uc, 0.0f, voltage_limit);
  // calculate duty cycle
  // limited in [0,1]
  dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
  dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
  dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );

  // hardware specific writing
  // hardware specific function - depending on driver and mcu
  // _writeDutyCycle3PWM(dc_a, dc_b, dc_c, params);

  if(timerA != nullptr)
  {
    timerA->SetPwmDutyP(pwmACh, dc_a);
  }
  if(timerB != nullptr)
  {
    timerB->SetPwmDutyP(pwmBCh, dc_b);
  }
  if(timerC != nullptr)
  {
    timerC->SetPwmDutyP(pwmCCh, dc_c);
  }

#endif 
}
